/******************************************************************
简介：实现Mega2560扩展版声明单个编码电机底层库
程序版本0.3
日期：2025.03.10
QQ交流群：811348489
寄语：代码中能遇见你，真好，有你，真好.
署名：有你同创智能机器人科技(北京)有限公司
******************************************************************/

/**********************************************************************************************************
软件：Arduino2.3.4
硬件：Mega2560，Mega2560扩展版
Mega2560扩展板丝印：
 _______________________________________________________________________________________________________________
|  Motor     |  丝印标签    |    对应程序标签    |                     对应Mega2560引脚号                          \
|            |_____________|___________________|_________________________________________________________________|
|            |     M1      |        1          |电机方向1:37  电机方向2:36  电机pwm:8   编码器A相:18  编码器B相:32  |
|            |     M2      |        2          |电机方向1:A2  电机方向2:A3  电机pwm:5   编码器A相:19  编码器B相:38  |
|            |     M3      |        3          |电机方向1:34  电机方向2:35  电机pwm:12  编码器A相:20  编码器B相:33  |
|            |     M4      |        4          |电机方向1:43  电机方向2:42  电机pwm:9   编码器A相:21  编码器B相:39  |
|            |     M5      |        5          |电机方向1:31  电机方向2:30  电机pwm:6   编码器A相:3   编码器B相:41  |
|            |     M6      |        6          |电机方向1:49  电机方向2:48  电机pwm:4   编码器A相:2   编码器B相:40  /
|￣￣￣￣￣￣`|￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣\
|  Servo     |     D2      |        无         |                            2                                    |
|            |     D3      |        无         |                            3                                    |
|            |     D4      |        无         |                            4                                    |
|            |     D6      |        无         |                            6                                    |
|            |     D44     |        无         |                            44                                   |
|            |     D45     |        无         |                            45                                   |
|            |     D46     |        无         |                            46                                   |
|            |     D7      |        无         |                            7                                    |
|            |     D10     |        无         |                            10                                   |
|            |     D11     |        无         |                            11                                   |
|            |     D13     |        无         |                            13                                   /
|￣￣￣￣￣￣`|￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣\
|Ultrasonic1 |     5V      |        无         |                            5V                                   |
|            |     D23     |        无         |                            23                                   |
|            |     D22     |        无         |                            22                                   |
|            |     GND     |        无         |                            GND                                  |
|            |￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣\
|Ultrasonic2 |     5V      |        无         |                            5V                                   |
|            |     D25     |        无         |                            25                                   |
|            |     D24     |        无         |                            24                                   |
|            |     GND     |        无         |                            GND                                  |
|            |￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣\
|Ultrasonic3 |     5V      |        无         |                            5V                                   |
|            |     D27     |        无         |                            27                                   |
|            |     D26     |        无         |                            26                                   |
|            |     GND     |        无         |                            GND                                  |
|            |￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣
|Ultrasonic4 |     5V      |        无         |                            5V                                   |
|            |     D29     |        无         |                            29                                   |
|            |     D28     |        无         |                            28                                   |
|            |     GND     |        无         |                            GND                                  |
|￣￣￣￣￣￣`|￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣\
| PS2        |     GND     |        无         |                            GND                                  |
|            |     5V      |        无         |                            5V                                   |
|            |     52      |        无         |                            SCK                                  |
|            |     51      |        无         |                            MOSI                                 |
|            |     53      |        无         |                            SS                                   |
|            |     50      |        无         |                            MISO                                 |
|￣￣￣￣￣￣`|￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣\
|Line sensor |     5V      |        无         |                            5V                                   |
|            |     GND     |        无         |                            GND                                  |
|            |     l       |        无         |                            A12                                  |
|            |     2       |        无         |                            A13                                  |
|            |     3       |        无         |                            A14                                  |
|            |     4       |        无         |                            A15                                  |
|￣￣￣￣￣￣`|￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣\
|COM3        |     5V      |        无         |                            5V                                   |
|            |     RX3     |        无         |                            RX3                                  |
|            |     TX3     |        无         |                            TX3                                  |
|            |     GND     |        无         |                            GND                                  |
|￣￣￣￣￣￣`|￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣\
|COM2        |     5V      |        无         |                            5V                                   |
|            |     RX2     |        无         |                            RX2                                  |
|            |     TX2     |        无         |                            TX2                                  |
|            |     GND     |        无         |                            GND                                  |
|￣￣￣￣￣￣`|￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣\
|IIC         |     5V      |        无         |                            5V                                   |
|            |     SCL     |        无         |                            SCL                                  |
|            |     SDA     |        无         |                            SDA                                  |
|            |     GND     |        无         |                            GND                                  |
|￣￣￣￣￣￣`|￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣\
|Stepper     |     EN      |        无         |                            30                                   |
|            |     MS1     |        无         |                            31                                   |
|            |     MS2     |        无         |                            49                                   |
|            |     MS3     |        无         |                            48                                   |
|            |     STEP    |        无         |                            2                                    |
|            |     DIR     |        无         |                            40                                   |
￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣￣

**********************************************************************************************************/
#ifndef _UNI_MOTOR_H_
#define _UNI_MOTOR_H_

#include <Arduino.h>
#include <Encoder.h>
#define MOTOR_LIB_VERSION 0.3//库函数版本(新增功能-编码电机位置模式pid控制)
#define UNI_MOTOR_1 1  //电机标签1
#define UNI_MOTOR_2 2  //电机标签2
#define UNI_MOTOR_3 3  //电机标签3
#define UNI_MOTOR_4 4  //电机标签4
#define UNI_MOTOR_5 5  //电机标签5
#define UNI_MOTOR_6 6  //电机标签6

#define UNI_WORK_VEL 1 //设定电机速度模式下的标签
#define UNI_WORK_POS 2 //设定电机位置模式下的标签
#define UNI_WORK_PWM 3 //设定电机PWM模式下的标签

#define UNI_MOTOR_VEL_MAX 1.0  //电机速度模式可以设定的顺时针转动最大速度
#define UNI_MOTOR_VEL_MIN -1.0 //电机速度模式可以设定的逆时针转动最大速度

class UNI_MOTOR{
  public:
  /*--------------------------------------------------------------------------------------------------------------
  功能：构造函数定义1. 用来声明编码电机对象.
  概述：该构造函数为使用本实验提供的编码电机，方便开发者使用
  函数原型：UNI_MOTOR(int motor_label);
  函数讲解：
      【显示的形式参数】：
        int motor_label： 表示电机标签
              如果motor_label=1:表示使用的编码电机引脚为：电机方向1:37   电机方向2:36   电机pwm:8   编码器A:18  编码器B:32
              如果motor_label=2:表示使用的编码电机引脚为：电机方向1:A2   电机方向2:A3   电机pwm:5   编码器A:19  编码器B:38
              如果motor_label=3:表示使用的编码电机引脚为：电机方向1:34   电机方向2:12   电机pwm:35  编码器A:20  编码器B:33
              如果motor_label=4:表示使用的编码电机引脚为：电机方向1:43   电机方向2:9    电机pwm:42  编码器A:21  编码器B:39
              如果motor_label=5:表示使用的编码电机引脚为：电机方向1:31   电机方向2:6    电机pwm:30  编码器A:3   编码器B:41
              如果motor_label=6:表示使用的编码电机引脚为：电机方向1:49   电机方向2:48   电机pwm:4   编码器A:2   编码器B:40
      【省略的参数】：
        float _motor_kp: 25.0
        float _motor_ki: 0.1
        float _motor_kd: 0.1
        float _motor_pulse: 1560 (本实验用的编码电机转动一圈，编码器计数为1560)
        float _motor_radius: 0.04米 (本实验用的编码电机采用默认的外轮直径为8厘米，故这里的半径为0.04米)  */
    UNI_MOTOR(int motor_label);

    /*--------------------------------------------------------------------------------------------------------------------------
    功能：构造函数定义4. 用来声明编码电机对象.
    概述：该构造函数可以使用L298N驱动的编码电机，可以使用自己的，或者是其他厂家提供的编码电机，方便开发者使用
    函数原型：UNI_MOTOR(int m_1, int m_2, int pwm, int e_a, int e_b);
    函数讲解：
      int m_1: 表示控制编码电机方向的引脚1
      int m_2: 表示控制编码电机方向的引脚2
      int pwm: 表示控制编码电机PWM脉宽调制的引脚
      int e_a: 表示编码电机编码器A相引脚
      int e_b: 表示编码电机编码器B相引脚
      【省略的参数】：
      float kp: 表示设定pid调控中的kp的值
      float ki: 表示设定pid调控中的ki的值
      float kd: 表示设定pid调控中的kd的值
      float pulse: 表示车轮转动一圈编码器触发的总次数.计算方式如下：
                  pulse = ppr(电机线圈数) * 2 * reduction(电机减速比) * 2
                  本实验中使用的编码电机脉冲数 pulse = 13 * 2 * 30 * 2 = 1560
      float _motor_radius: 0.04米 (本实验用的编码电机采用默认的外轮直径为8厘米，故这里的半径为0.04米)  */
      UNI_MOTOR(int m_1, int m_2, int pwm, int e_a, int e_b);

    /*--------------------------------------------------------------------------------------------------------------------------
    功能：构造函数定义4. 用来声明编码电机对象.
    概述：该构造函数可以使用L298N驱动的编码电机，可以使用自己的，或者是其他厂家提供的编码电机，方便开发者使用
    函数原型：UNI_MOTOR(int m_1, int m_2, int pwm, int e_a, int e_b, float kp, float ki, float kd, float pulse, float radius, bool revs);
    函数讲解：
      int m_1: 表示控制编码电机方向的引脚1
      int m_2: 表示控制编码电机方向的引脚2
      int pwm: 表示控制编码电机PWM脉宽调制的引脚
      int e_a: 表示编码电机编码器A相引脚
      int e_b: 表示编码电机编码器B相引脚
      float kp: 表示设定pid调控中的kp的值
      float ki: 表示设定pid调控中的ki的值
      float kd: 表示设定pid调控中的kd的值
      float pulse: 表示车轮转动一圈编码器触发的总次数.计算方式如下：
                  pulse = ppr(电机线圈数) * 2 * reduction(电机减速比) * 2
                  本实验中使用的编码电机脉冲数 pulse = 13 * 2 * 30 * 2 = 1560
      float _motor_radius: 0.04米 (本实验用的编码电机采用默认的外轮直径为8厘米，故这里的半径为0.04米)  */
    UNI_MOTOR(int m_1, int m_2, int pwm, int e_a, int e_b, float kp, float ki, float kd, float pulse, float radius);


    ~UNI_MOTOR();                           //析构函数定义. 用来程序结束后,删除声明的电机.该函数目前无实际作用，后期可能会考虑重构该函数.
    void  uni_motor_begin();                //电机参数初始化设置,包含设置编码电机pid算法中需要用到的相关参数.
    void  uni_common_mode(int choose);      //电机重构函数中，部分电机参数设置相同的部分
    void  motor_pin_init();                 //设置编码电机方向，PWM脉宽引脚初始化
    void  set_parameters(float ppr, float reduction, float radius); //设置编码电机线圈数，减速比，轮子半径
    void  init();                           //电机引脚初始化设置，以及准备开始新的控制周期
    void  tic();                            //tic 方法更新当前时间戳 currentTime 和当前编码器读数 currentEnc
    void  toc();                            //toc 方法更新 lastTime 和 lastEnc 为当前值，以便在下一次调用 tic 时使用

    /*常用函数-------------------------------------------------------------------------*/
    float set_vel(float target_vel);        //设定编码电机需要到达的指定速度，单位：m/s
    void  set_pwm(int pwm);                 //设置电机速度.pwm模式，范围[-255 ~ 255]
    void  update();                         //刷新电机状态参数函数
    void  set_pos(float target_pos);        //设定编码电机需要到达的位置


    /*不常用函数-----------------------------------------------------------------------*/
    float get_version();                    //获取电机库版本号
    void  speed_loop();                     //封装编码电机速度环pid控制函数
    void  pos_loop();                       //封装编码电机位置环-速度环双环pid控制函数
    void  update_pos_encoder();             //更新电机位置模式下的编码电机计数值
    float getDeltaT();                      //获取编码电机计算周期
    long  get_encoder();                    //获取编码电机编码器读取到的当前数值
    float get_pos();                        //获取编码电机当前位置函数
    float get_current_vel();                //获取电机当前线速度
    float get_target_vel();                 //获取编码电机需要到达的目标线速度
    void  get_parameters();                 //打印当前电机参数信息,包含了当前编码电机的引脚，编码传感器引脚，pid参数,脉冲数，车轮半径
    void  pos_parameters_reset();           //位置模式下，复位位置模式参数
    void  keep_current_pos();               //编码电机运行到指定位置后，使用该函数是编码电机保持在当前位置
    void  set_vel_pid(float kp, float ki, float kd); //重新修改速度环模式pid参数值，如果没有调用该函数，则速度环pid为默认值
    void  set_pos_pid(float kp, float ki, float kd); //重新修改位置环模式pid参数值，如果没有调用该函数，则位置环pid为默认值
    unsigned int pos_flag = 1;              //编码电机是否退出双环pid控制模式的标志,如果该值为0表示退出双环pid控制，否则，表示未退出

  private:
    //编码电机硬件参数，包含电机引脚，编码器引脚，车轮转动一圈编码器触发的次数，车轮半径参数
    Encoder *enc;                               //指向编码器对象的指针，编码器通常用于测量电机的位置或速度
    int   _motor_pin1;                          //保存控制电机方向引脚1
    int   _motor_pin2;                          //保存控制电机方向引脚2
    int   _motor_pwm;                           //保存控制电机调速pwm引脚
    int   _motor_enca;                          //保存电机编码器A相引脚
    int   _motor_encb;                          //保存电机编码器B相引脚
    float _motor_pulse;                         //保存编码电机一圈脉冲数值
    float _motor_radius;                        //保存编码电机减速比


    //设定编码电机单环(速度环), 双环(位置环-速度环)需要定义的全局参数
    float  _motor_deltaT = 0.0;                 //时间间隔,0.1s
    unsigned long _motor_lastTime = 0;          //记录上次的时间戳
    unsigned long _motor_currentTime = 0;       //记录当前的时间戳
    unsigned int _motor_working_mode;           //记录电机当前运行模式.
    float  _motor_resolution = 0.0;             //记录编码电机车轮分辨率
    float  _motor_target_position = 0.0;        //设定电机需要到达的位置
    float  _motor_deltaEnc = 0.0;               //编码器读数的变化量
    float  _motor_currentEnc = 0.0;             //记录当前的编码器读数
    float  _motor_lastEnc = 0.0;                //记录上次的编码器读数
    float  _current_encoder_pos = 0.0;          //计算得到编码电机当前位置(转换为距离)
    float  _motor_target_encoder_pos = 0.0;     //计算编码电机需要转动到的编码器位置(转换为距离)
    float  _motor_reverse;                      //保存电机编码器标志，true:表示电机正转编码器取正 false：表示电机正转编码器取反
    unsigned int pos_vel_count = 0;             //判断编码电机是否成功到达指定位置的int类型次数判断

    //编码电机外环位置环相关参数设置，具体参数如下所示：
    float  _out_kp = 10.5;                       //外环位置环kp值
    float  _out_ki = 0.0;                       //外环位置环ki值
    float  _out_kd = 0.001;                      //外环位置环kd值
    float  _out_pos_error = 0.0;                //外环位置环误差
    float  _out_integral_pos = 0.0;             //外环位置积分项
    float  _out_derivative_pos = 0.0;           //外环位置微分项
    float  _out_pos_lasterror = 0.0;            //外环位置环上一次误差
    float  _out_output_vel = 0.0;               //外环位置环得到的速度输出
    float  _out_pos_delta_error = 0.005;        //外环位置环编码电机到达目标位置的误差，单位：米
    float  _save_all_encoder_data = 0.0;        //记录编码电机总值
    float  _save_encoder_current_pos = 0.0;     //存储编码电机当前位置变量

    //编码电机速度环相关参数设置，具体参数如下所示：
    float  _motor_current_wheel_vel = 0.0;      //(单环速度环/内环速度环)记录电机当前车轮速度值
    float  _motor_bias;                         //(单环速度环/内环速度环)当前速度差值偏差
    float  _motor_ki_error_sum;                 //(单环速度环/内环速度环)积分累计误差项
    float  _vel_derivative = 0.0;               //(单环速度环/内环速度环)微分项
    float  _motor_output_pwm = 0.0;             //(单环速度环/内环速度环)输出电机pwm值
    float  _motor_last_bias;                    //(单环速度环/内环速度环)上一次速度差值偏差值
    float  _motor_kp = 100.0;                   //(单环速度环/内环速度环)的kp的值
    float  _motor_ki = 0.0;                     //(单环速度环/内环速度环)的ki的值
    float  _motor_kd = 500.0;                   //(单环速度环/内环速度环)的kd的值
    int    _motor_pwm_max = 255;                //(单环速度环/内环速度环)pwm最大值
    int    _motor_pwm_min = -255;               //(单环速度环/内环速度环)pwm最小值
    float  _motor_vel_pid_max = 0.2;            //(单环速度环/内环速度环)编码电机能到达的最大速度，单位:m/s
    float  _motor_vel_pid_min = -0.2;           //(单环速度环/内环速度环)编码电机能到达的最小速度，单位:m/s
    float _motor_target_wheel_vel = 0.0;        //(单环速度环/内环速度环)设定编码电机需要到达的线速度，单位:m/s
};

#endif
